﻿using System;
using System.Text;
using System.Threading;
using MLRobotic.IA.Tools;

namespace MLRobotic.Asserv
{
    public delegate double GetDouble();
    public delegate void SetDouble(double value);
    public class PID
    {

        private static System.Threading.Timer ti;

        //Threading and Timing
        public static readonly int computeDelay = 25;

        public static PID pidAvance;
        public static PID pidAngle;


        #region Fields
        //Gains
        private double kp;
        private double ki;
        private double kd;

        //Running Values
        private DateTime lastUpdate;
        private double lastPV;
        private double errSum;

        //Reading/Writing Values
        private GetDouble readPV;
        private GetDouble readSP;
        private SetDouble writeOV;

        //Max/Min Calculation
        private double pvMax;
        private double pvMin;
        private double outMax;
        private double outMin;

        #endregion

        #region Properties

        public double PGain
        {
            get { return kp; }
            set { kp = value; }
        }

        public double IGain
        {
            get { return ki; }
            set { ki = value; }
        }

        public double DGain
        {
            get { return kd; }
            set { kd = value; }
        }

        public double PVMin
        {
            get { return pvMin; }
            set { pvMin = value; }
        }

        public double PVMax
        {
            get { return pvMax; }
            set { pvMax = value; }
        }

        public double OutMin
        {
            get { return outMin; }
            set { outMin = value; }
        }

        public double OutMax
        {
            get { return outMax; }
            set { outMax = value; }
        }

        public double ErrSum
        {
            get { return errSum; }
            set { errSum = value; }
        }

        #endregion

        #region Construction / Deconstruction

        public PID(double pG, double iG, double dG,
            double oMax, double oMin,
            GetDouble pvFunc, GetDouble spFunc, SetDouble outFunc)
        {
            kp = pG;
            ki = iG;
            kd = dG;
            outMax = oMax;
            outMin = oMin;
            readPV = pvFunc;
            readSP = spFunc;
            writeOV = outFunc;
        }

        ~PID()
        {
            Disable();
            readPV = null;
            readSP = null;
            writeOV = null;
        }

        #endregion

        #region Public Methods

        public static void Enable()
        {
            ti = new System.Threading.Timer(new TimerCallback(Tick), null, computeDelay, computeDelay);
        }

        public static void Reset()
        {
            if (pidAvance != null)
                pidAvance.ResetVal();
            if (pidAngle != null)
                pidAngle.ResetVal();
        }
        private void ResetVal()
        {
            errSum = 0;
            lastPV = 0;
        }

        private static void Tick(object sender)
        {
            if (pidAvance != null)
                pidAvance.Compute();
            if (pidAngle != null)
                pidAngle.Compute();
        }

        public static void Disable()
        {
            ti.Dispose();
        }


        #endregion

        #region Private Methods

        private double ScaleValue(double value, double valuemin,
                double valuemax, double scalemin, double scalemax)
        {
            double vPerc = (value - valuemin) / (valuemax - valuemin);
            double bigSpan = vPerc * (scalemax - scalemin);

            double retVal = scalemin + bigSpan;

            return retVal;
        }

        private double Clamp(double value, double min, double max)
        {
            if (value > max)
                return max;
            if (value < min)
                return min;
            return value;
        }

        private void Compute()
        {
            // récupération la valeur réelle obtenue par les codeurs
            double pv = readPV();
            // récupérer la valeur désirée
            double sp = readSP();

            //calcul d'erreur
            double err = sp - pv;

            // P
            double pTerm = err * kp;

            double iTerm = 0.0f;
            double dTerm = 0.0f;

            errSum += err;

            //calculPIDSommeErreur += Math2.Abs(err);

            // I
            iTerm = ki * errSum;

            // D
            dTerm = kd * (pv - lastPV);


            lastPV = pv;

            // Somme des valeures
            double outReal = pTerm + iTerm + dTerm;

            // écrétage des valeurs maximum
            outReal = Clamp(outReal, outMin, outMax);


            // Valur de sortie
            writeOV(outReal);
        }

        #endregion

        public double calculPIDSommeErreur = 0;
    }
}
